#!/usr/bin/env python3
import os
import sys
import rclpy
from rclpy.node import Node
from rclpy.executors import MultiThreadedExecutor
from rclpy.callback_groups import ReentrantCallbackGroup
from ament_index_python.packages import get_package_share_directory
import datetime
import time
import copy
import json
import jsonschema
import paho.mqtt.client as mqtt
import yaml
import base64
import cv2
import numpy as np

from dataclasses import dataclass, field
from dotmap import DotMap
from typing import List, Dict

# ROS2 Messages
from agv_msgs.msg import (
    Action,
    ActionParam,
    AGVEvent,
    AGVEventStatus,
    AGVMode,
    AGVPosition,
    AgvAutoStatus,
    BatInfo,
    ConnectionMonitor,
    Diagnosis,
    EdgeState,
    Load,
    MapEditErase,
    MapEditPoints,
    MapEditPoly,
    MapEditTransform,
    MapNode,
    PeriphStatus,
    PointCloudPubSwitch,
    RelocateRadius,
    State,
    Velocity
)

from agv_srvs.srv import (
    Order, InstantActions, Relocate, LandmarkMap, SaveMap, SetLoactionMode,
    UpdateQr, EditMap, StartMapMerge
)

# ROS2 Messages (standard)
from sensor_msgs.msg import PointCloud2
from nav_msgs.msg import OccupancyGrid
from geometry_msgs.msg import Point
from std_srvs.srv import Empty

ORDER_TOPIC = 'order'
INSTANT_ACTION = 'instantActions'
STATE_TOPIC = 'state'
CONNECTION_TOPIC = 'connection'
LOCALIZATION_TOPIC = 'localization'
QR_LOCALIZATION_REQUEST_TOPIC = 'qrLocalizationRequest'
QR_LOCALIZATION_RESPONSE_TOPIC = 'qrLocalizationResponse'

POINT_CLOUD_SWITCH = 'pointCloudSwitch'
SAVE_MAP_TOPIC = 'saveMap'
SAVE_MAP_RESPONSE_TOPIC = 'saveMapResponse'
NAV_MODE_TOPIC = 'navMode'
NAV_MODE_RESPONSE_TOPIC = 'navModeResponse'

MAP_TOPIC = 'map'
POINT_CLOUD_TOPIC = 'pointCloud'
MAP_MODIFY_TOPIC = 'mapModify'
MAP_MODIFY_RESPONSE_TOPIC = 'mapModifyResponse'
MAP_MERGE_TOPIC = 'mapMerge'
MAP_MERGE_RESPONSE_TOPIC = 'mapMergeResponse'
UPDATEQR_TOPIC = 'updateQR'
UPDATEQR_RESPONSE_TOPIC = 'updateQrResponse'

ORDER_ID_TOPIC = 'orderIdTopic'

# Schema consts
ORDER_SCHEMA = 'order.schema'
INSTANT_ACTION_SCHEMA = 'instantActions.schema'
STATE_SCHEMA = 'state.schema'
CONNECTION_SCHEMA = 'connection.schema'

# Operating modes
AUTOMATIC_OP_MODE = 'AUTOMATIC'
SEMIAUTOMATIC_OP_MODE = "SEMIAUTOMATIC"
MANUAL_OP_MODE = "MANUAL"
SERVICE_OP_MODE = "SERVICE"
TEACHIN_OP_MODE = "TEACHIN"

# Action status
ACTION_WAITING = 'WAITING'
ACTION_INITIALIZING = 'INITIALIZING'
ACTION_RUNNING = 'RUNNING'
ACTION_PAUSED = 'PAUSED'
ACTION_FINISHED = 'FINISHED'
ACTION_FAILED = 'FAILED'

@dataclass
class nodeState():
    nodeId: str = None
    sequenceId: str = None
    released: bool = None
    nodePosition: Dict[float, str] = field(default_factory = lambda: {'x': 0.0, 'y': 0.0, 'theta': 0.0, 'allowedDeviationXy': 0.0, 'allowedDeviationXy': 0.0, 'mapId': '', 'mapDescription': ''})
    released: bool = None

@dataclass
class edgeState():
    edgeId: str = None
    sequenceId: str = None
    released: bool = None

@dataclass
class actionState():
    actionId: str = None
    actionType: str = None
    actionStatus: str = None # waiting, initializing, running, finished and failed
    #action_parameter: Dict[str, float] = field(default_factory= lambda: {'key': '', 'value': ''})
    actionDescription: str = None
    resultDescription: str = None

@dataclass
class load():
    loadId: str = None
    loadType: str = None
    loadPosition: str = None
    boundingBoxReference: Dict[float,str] = field(default_factory = lambda:{'x': 0.0, 'y': 0.0, 'z':0.0,'theta': 0.0})
    loadDimensions: Dict[float,str] = field(default_factory = lambda:{'length': 0.0, 'width': 0.0, 'height':0.0})
    weight: float = None

@dataclass
class batteryState():
    batteryCharge: float = None
    charging: bool = None

@dataclass
class pc_switch():
    pc_pub_switch: bool = None
    map_pub_switch: bool = None

@dataclass
class _errorReferences():
    referenceKey: str = None
    referenceValue: str = None

@dataclass
class _errors():
    errorType: str = None
    errorReferences: List[_errorReferences] = None
    errorDescription: str = None
    errorLevel: str = None

@dataclass
class events():
    occur_id: int = None
    event_details: str = None

@dataclass
class state():
    headerId: int = None
    orderId: str = None
    orderUpdateId: int = None
    timestamp: str = None
    version: str = None
    serialNumber:str = None
    manufacturer: str = None
    lastNodeId: str = None
    lastNodeSequenceId = None
    driving: bool = True
    operatingMode: str = None # (Enum) only certain strings are accepted.
    #batteryStates: batteryState = None
    batteryStates:Dict[float, str] = field(default_factory = lambda: {'batteryCharge': 0.0, 'batteryVoltage': 0.0, 'batteryHealth': 0.0,'charging':False,'chargeFinished':False,'reach':0.0})
    velocity: Dict[float, str] = field(default_factory = lambda: {'vx': 0.0, 'vy': 0.0, 'omega': 0.0})
    agvPosition: Dict[float, str] = field(default_factory = lambda: {'x': 0.0, 'y': 0.0, 'theta': 0.0, 'mapId': '', 'mapDescription': '', "positionInitialized": True, "localizationScore":0.0, "deviationRange":0.0})
    nodeStates: List[nodeState] = None # no use
    edgeStates: List[edgeState] = None # no use
    #actionStates: List[actionState] = None
    actionStates: List[Dict] = None
    paused: bool = False
    distanceSinceLastNode: float = None
    #errors = List[_errors]
    errors: List[Dict] = None
    zoneSetId: str = None
    newBaseRequest: bool = True
    loads: List[load] = None
    #event: events = None
    agvEvents: Dict[str,int] = field(default_factory = lambda: {'occur_id': 0, 'event_details': ''})
    #agvEvents: List[Dict] = None
    #description:str = None
    center_mileage: float = None
    online_time: float = None
    move_time: float = None
    fault_time: float = None
    order_counts: int = None
    discharge_time: float = None
    charge_counts: int = None
    charge_time: float = None

class TaskManager(Node):
    def __init__(self):
        super().__init__('VDA5050Protocol')
        pkg_share = get_package_share_directory('agv_dispatch_comm')

        # 获取配置路径
        config_path = os.path.join(
            pkg_share,
            'config',
            'vda_config.yaml'
        )

        self.schemas_path = pkg_share + "/schemas/"
        self.msg_path = pkg_share + "/messages/"

        self.declare_parameters(
            namespace='',
            parameters=[
                ('broker_ip', '192.168.0.240'),
                ('broker_port', 1883),
                ('net_interval', 15),
                ('localization', 1)
            ]
        )
        
        with open(config_path) as fp:
            self.config = yaml.safe_load(fp)
            # self.schemas_path = os.path.expanduser(self.config["schemas_path"])
            # self.msg_path = os.path.expanduser(self.config["msg_path"])
        
        self.agv_num = os.getenv('ROBOT_ID', 'default_agv')
        self.broker_ip = self.get_parameter('broker_ip').value
        self.broker_port = self.get_parameter('broker_port').value
        self.net_interval = self.get_parameter('net_interval').value

        # MQTT 客户端初始化
        self.mqtt_client = mqtt.Client(f'Task Manager'+self.agv_num)
        self.mqtt_client.on_message = self.mqtt_cb
        self.mqtt_client.on_connect = self.on_connect
        self.mqtt_client.on_disconnect = self.on_disconnect

        self.time_monitor_old = datetime.datetime.now()
        self.broker_disconnect = 1
        self.state = state()

        self.point_msg = {}
        self.pc_pub_switch = False #True #False
        self.map_pub_switch = False #True #False
        self.subscribe()
        self.publish()
        self.create_timer(1.0, self.publish_connection_state)

        try:
            result = self.mqtt_client.connect(self.broker_ip,self.broker_port) #connect to broker
            while result != 0:
                result = self.mqtt_client.reconnect()
                if result == 0:
                    break

                time.sleep(1)
        except Exception as e:
            self.get_logger().error(str(e))
            self.get_logger().error("MQTT connection failed!")
            while not rclpy.ok():
                self.get_logger().warn("MQTT reconnecting!")
                try:
                    time.sleep(1)
                    result = self.mqtt_client.reconnect()
                    if result == 0:
                        break
                except:
                    self.get_logger().error("reconnect except")
                    pass

        self.mqtt_client.loop_start()
        self.description = ''
        # create initial states for orderId, orderUpdateId, order_active
        self.order_prev_id = ''
        self.order_update_id = 0
        self.order_active = True # boolean
        self.header_id = 0 # This value is incremented every time a message is sent but is reset when the program is restarted
        self.wait_trigger = False

        # create the classes for storing all robot state information

        #self.state.batteryStates = batteryState()
        #self.state.batteryStates = dict()
        self.state.last_node_id = '0'

        self.state.serialNumber = self.agv_num

        #self.time_monitor_old = datetime.datetime.now()
        self.time_monitor_new = None

        self.master_disconnect = 1
        #self.broker_disconnect = 0
        
        self.point_list = []
        self.orderIdMsg = {'orderIdNew':'','orderUpdateIdNew':0,'nodeIdNew':''}
        self.point_msg = {'stamp':0,'points':[]}
        self.map_msg = {'stamp':0,'resolution':0.0,'width':0,'height':0,'origin':{'position':{'x':0.0,'y':0.0},'orientation':{'x':0.0,'y':0.0,'z':0.0,'w':1.0}},'data':[]}
        # self.pc_pub_switch = False #True #False
        # self.map_pub_switch = False #True #False
        self.i = 0
        self.send_info = None
        self.navigation_mode = None
        self.qrLocation = {}
        self.map_modify_ret_json = None
        self.heart_bit_state = None
        self.seq = 0
        self.t_old = datetime.datetime.now()

        with open(self.msg_path +'state_msg.json', 'r', encoding = 'utf8') as json_file:
            # Open json schema as a template for creating the order.
            self.json_object = json.load(json_file)
            json_file.close()

        time.sleep(1)

        self.orderClient = self.create_client(Order, "vda5050_order")
        self.instantOrderClient = self.create_client(InstantActions, "vda5050_instantActions")
        self.relocateClient = self.create_client(Relocate, "agv_relocate")
        self.QRlocateClient = self.create_client(LandmarkMap, "landmark_service")
        self.saveMapClient = self.create_client(SaveMap, "save_map")
        self.navModeClient = self.create_client(SetLoactionMode, "location_mode")
        self.mapEditClient = self.create_client(EditMap, "edit_map")
        self.updateQR_client = self.create_client(UpdateQr, "updateqr")
        self.mapMerge_client = self.create_client(StartMapMerge, "merge_map")

        # 定时器
        self.create_timer(0.2, self.publish_state)
        self.create_timer(1.0, self.publish_order_id)
        self.create_timer(0.1, self.state_json)
        self.create_timer(1.0, self.publish_point_cloud)
        self.create_timer(1.0, self.publish_map)
        self.create_timer(1.0, self.publish_error)

    def robot_feedback(self, data):
        pass

    def on_disconnect(self,data,rc,properties=None):
        if rc != 0:
            self.broker_disconnect = True
            self.get_logger().warn("Connected to MQTT Broker failed! rc:" + str(rc))
        else:
            self.get_logger().info("Broker connection is normal!")
     
    def on_connect(self, data, flags, rc,properties=None):
        if rc['session present'] == 0:
            #self.mqtt_client.subscribe([(ORDER_TOPIC, 0), (INSTANT_ACTION, 0),(CONNECTION_TOPIC, 0)])
            self.mqtt_client.subscribe([('/'+self.agv_num +'/'+ ORDER_TOPIC, 0), \
                ('/'+self.agv_num +'/' + INSTANT_ACTION, 0),\
                ('/'+self.agv_num+'/' + CONNECTION_TOPIC, 0),\
                ('/'+self.agv_num +'/' + LOCALIZATION_TOPIC,0),\
                ('/'+self.agv_num +'/' + QR_LOCALIZATION_REQUEST_TOPIC,0),\
                ('/'+self.agv_num +'/' + POINT_CLOUD_SWITCH,0),\
                ('/'+self.agv_num +'/' + SAVE_MAP_TOPIC,0),\
                ('/'+self.agv_num +'/' + NAV_MODE_TOPIC,0),\
                ('/'+self.agv_num +'/' + MAP_MODIFY_TOPIC,0),\
                ('/'+self.agv_num +'/'+ UPDATEQR_TOPIC,0),\
                ('/'+self.agv_num +'/'+ MAP_MERGE_TOPIC,0)\
                ])
            self.broker_disconnect = False
            self.get_logger().info("Connected to MQTT Broker!")
        else:
            self.get_logger().info("Failed to connect!")

    def subscribe(self):
        self.create_subscription(State, 'vda5050_state', self.state_cb, 10)
        self.create_subscription(AGVMode, 'agvmode', self.agv_mode_cb, 10)
        self.create_subscription(BatInfo, 'batteryInfo', self.battery_info_cb, 10)
        self.create_subscription(PointCloud2, '/scan_matched_points2', self.scan_cb, 10)
        self.create_subscription(OccupancyGrid, '/map', self.map_cb, 10)
        self.create_subscription(PointCloudPubSwitch, '/point_cloud_switch', self.switch_cb, 10)
        self.create_subscription(AGVEventStatus, '/eventstatus', self.event_cb, 10)
        self.create_subscription(PeriphStatus, '/periphStatus', self.periphStatus_cb, 10)
        self.create_subscription(AGVEvent, '/agvevent', self.agv_event_cb, 10)
        self.create_subscription(AgvAutoStatus, '/auto_run_status', self.agv_status_cb, 10)
        self.create_subscription(Diagnosis, '/statistics_data', self.agv_diagnosis_cb, 10)
        self.create_subscription(PointCloudPubSwitch, '/navigation_mode', self.navigation_mode_cb, 10)
        
    def publish(self):
        self.connection_pub = self.create_publisher(ConnectionMonitor, 'connection_state', 10)
        self.pc_switch_pub = self.create_publisher(PointCloudPubSwitch, '/point_cloud_switch', 1)
        self.error_pub = self.create_publisher(AGVEvent, '/agvevent', 1)

    def publish_state(self):
        if self.heart_bit_state:
            self.mqtt_client.publish(f'/{self.agv_num}/{STATE_TOPIC}', self.heart_bit_state)

    def publish_connection_state(self):
        # self.get_logger().info("publish_connection_state")
        self.time_monitor_new = datetime.datetime.now()
        dt = abs(self.time_monitor_new - self.time_monitor_old)
        if dt.total_seconds() >= self.net_interval:
            self.get_logger().info("connection to master failed!!! interval:" + str(dt.total_seconds()))
            self.master_disconnect = 1
        else:
            self.master_disconnect = 0
        connection_state = ConnectionMonitor()
        connection_state.brokerconnnection = self.broker_disconnect
        connection_state.masterconnnection = self.master_disconnect
        self.connection_pub.publish(connection_state)

    def battery_info_cb(self,msg):
        self.state.batteryStates['batteryCharge'] = msg.numeric_bat
        self.state.batteryStates['charging'] = msg.chargefflag
        self.state.batteryStates['batteryVoltage'] = msg.voltage
        self.state.batteryStates['chargeFinished'] = msg.chargefinished
        #self.get_logger().info(self.state.batteryStates.batteryCharge)
        #
    def navigation_mode_cb(self,msg):
        self.navigation_mode = msg.mode

    def agv_diagnosis_cb(self,msg):
        self.state.center_mileage = msg.center_mileage
        self.state.online_time = msg.online_time
        self.state.move_time = msg.move_time
        self.state.fault_time = msg.fault_time
        self.state.order_counts = msg.order_counts
        self.state.discharge_time = msg.discharge_time
        self.state.charge_counts = msg.charge_counts
        self.state.charge_time = msg.charge_time


    def agv_status_cb(self,msg):
        #print(msg)
        self.t_now = datetime.datetime.now()
        self.orderIdMsg['orderIdNew'] = msg.orderidnew
        self.orderIdMsg['orderUpdateIdNew'] = msg.orderupdateidnew
        self.orderIdMsg['nodeIdNew'] = msg.nodeidnew
        self.get_logger().info("receive order finish! take " + str(self.t_now-self.t_old), throttle_duration_sec=0.5)
        self.t_old = self.t_now
        
    def publish_order_id(self):
        try:
            self.idMsg = json.dumps(self.orderIdMsg)
            ret = self.mqtt_client.publish('/'+ self.agv_num +'/'+ ORDER_ID_TOPIC, self.idMsg)
            self.get_logger().info("pub orderId return at " + str(datetime.datetime.now()), throttle_duration_sec=0.5)
            #print(self.orderIdMsg)
        except Exception as e:
            self.get_logger().error(str(e))


    def agv_event_cb(self,msg):
        #print((msg.header.frame_id))
        if msg.header.frame_id == "/qrodom":
            self.description = msg.description
            #print(msg.description + "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")

    # 2023.12.19 modify by haibo
    def agv_mode_cb(self,msg):
        if msg.mode == msg.MANAUL:
            self.state.operatingMode = MANUAL_OP_MODE
        if msg.mode == msg.SEMI_AUTOMATIC:
            self.state.operatingMode = SEMIAUTOMATIC_OP_MODE
        if msg.mode == msg.AUTOMATIC:
            self.state.operatingMode = AUTOMATIC_OP_MODE
        if msg.mode == msg.LOCAL_AUTOMATIC:
            self.state.operatingMode = TEACHIN_OP_MODE

    def state_cb(self,msg):
        self.state.orderId = msg.orderid
        self.state.orderUpdateId = msg.orderupdateid
        self.state.lastNodeId = msg.lastnodeid
        self.state.lastNodeSequenceId = msg.lastnodesequenceid
        self.state.agvPosition['x'] = msg.agvposition.x
        self.state.agvPosition['y'] = msg.agvposition.y
        self.state.agvPosition['theta'] = msg.agvposition.theta
        self.state.agvPosition['localizationScore'] = msg.agvposition.localizationscore
        self.state.agvPosition['mapId'] = msg.agvposition.mapid
        self.state.velocity['vx'] = msg.velocity.vx
        self.state.velocity['vy'] = msg.velocity.vy
        self.state.velocity['omega'] = msg.velocity.omega
        #self.state.serialNumber = msg.serialNumber
        self.state.driving = msg.driving
        self.state.paused = msg.paused
        self.state.distanceSinceLastNode = msg.distancesincelastnode


        #actionState_ = actionState()
        actionState_list = []
        for aState in msg.actionstates:
            actionState_ = {'actionId': '', 'actionType': '', 'actionStatus': '', 'actionDescription':'','resultDescription':''}
            #actionState_ = {}
            actionState_['actionId'] = aState.actionid
            actionState_['actionType'] = aState.actiontype
            actionState_['actionStatus'] = aState.actionstatus
            actionState_['actionDescription'] = aState.actiondescription
            actionState_['resultDescription'] = aState.resultdescription
            actionState_list.append(copy.deepcopy(actionState_))
        self.state.actionStates = actionState_list

        edgeState_list = []
        for edgeState in msg.edgestates:
            edgeState_ = {'edgeId':'','sequenceId':'','edgeDescription':'','released':''}
            edgeState_['edgeId'] = edgeState.edgeid
            edgeState_['sequenceId'] = edgeState.sequenceid
            edgeState_['edgeDescription'] = edgeState.edgedescription
            edgeState_['released'] = edgeState.released
            edgeState_list.append(copy.deepcopy(edgeState_))
        self.state.edgeStates = edgeState_list


    def periphStatus_cb(self,msg):
        loads_list = []
        for load in msg.loads:
            load_ = {'loadId':'','loadType':'-1','loadPosition':'',"boundingBoxReference":{"x":0.0,"y":0.0,"z":0.0,"theta":0.0},"loadDimensions":{"length":0.0,"width":0.0,"height":0.0},"weight":0.0}
            load_['loadPosition']=load.loadposition
            load_['loadId']=load.loadid
            if load.loadtype != '':
                load_['loadType']=load.loadtype
            loads_list.append(copy.deepcopy(load_))
        self.state.loads = loads_list


    def new_order(self, order):
        pass

    def change_order(self, order):
        # 更新状态头信息
        self.state.headerId = self.header_id
        self.state.manufacturer = order.manufacturer

        # 构建节点和动作列表
        v_nodeList = []
        for node in order.nodes:
            v_node = MapNode(
                nodeid=node.nodeId,
                sequenceid=node.sequenceId,
                released=node.released,
                actions=[]
            )
            for action in node.actions:
                action_parameters = [
                    ActionParam(key=param.key, value=param.value)
                    for param in action.actionParameters
                ]
                v_action = Action(
                    actiontype=action.actionType,
                    actionid=action.actionId,
                    blockingtype=action.blockingType,
                    actionparameters=action_parameters
                )
                v_node.actions.append(v_action)
            v_nodeList.append(v_node)

        try:
            self.get_logger().warn("Waiting for service: vda5050_order...")
            if not self.orderClient.wait_for_service(timeout_sec=5.0):
                self.get_logger().error("Service 'vda5050_order' not available!")
                return
            orderReq = Order.Request()
            orderReq.order.zonesetid = order.zoneSetId
            orderReq.order.timestamp = order.timestamp  # 
            orderReq.order.serialnumber = order.serialNumber
            orderReq.order.orderid = order.orderId
            orderReq.order.orderupdateid = order.orderUpdateId
            orderReq.order.nodes = v_nodeList

            self.get_logger().info(
                f"Sending order: ID={order.orderId}, "
                f"Nodes={len(v_nodeList)}, Actions={sum(len(n.actions) for n in v_nodeList)}"
            )

            t1 = self.get_clock().now()
            future = self.orderClient.call_async(orderReq)
            rclpy.spin_until_future_complete(self, future, timeout_sec=10.0)

            if not future.done():
                self.get_logger().error("Service call timed out!")
                return

            t2 = self.get_clock().now()
            self.get_logger().info(f"Service call took {(t2 - t1).nanoseconds / 1e9:.2f}s")

            response = future.result()
            if response.success:
                self.get_logger().info("Order successfully sent!")
            else:
                self.get_logger().error(f"Order failed: {response.message}")

        except rclpy.exceptions.ServiceException as e:
            self.get_logger().error(f"Service error: {str(e)}")
        except Exception as e:
            self.get_logger().error(f"Unexpected error: {str(e)}")

    def order_handle(self, order):
        try:
            order_mapped = DotMap(order)
        except Exception as e:
            self.get_logger().error("DotMap error!!!")
            self.get_logger().error(str(e))

        self.change_order(order_mapped)

    def instant_action_handle(self, instant_order):
        #order_mapped = DotMap(instant_order)
        #self.get_logger().info(instant_order)
        v_instantActionsList = []
        v_ainstantAction = Action()
        actionParameters = []
        try:
            for instantAction in instant_order['actions']:
                for param in instantAction['actionParameters']:
                    actionParam = ActionParam()
                    actionParam.key   = param['key']
                    actionParam.value = param['value']
                    actionParameters.append(copy.deepcopy(actionParam))
                v_ainstantAction.actionparameters = actionParameters
                v_ainstantAction.actiontype = instantAction['actionType']
                v_ainstantAction.actionid = instantAction['actionId']
                v_ainstantAction.blockingtype = instantAction['blockingType']
                v_instantActionsList.append(copy.deepcopy(v_ainstantAction))

        except Exception as e:
            self.get_logger().error(str(e))

        try:
            if not self.instantOrderClient.wait_for_service(timeout_sec=5.0):
                self.get_logger().error("Service 'vda5050_instantActions' not available")
                return

            # self.node.wait_for_service("vda5050_instantActions", 2.0)
            instant_orderRequest = InstantActions.Request()
            instant_orderRequest.actions = v_instantActionsList
            self.get_logger().info("call instant_action!!!")
            t1 = datetime.datetime.now()
            future = self.instantOrderClient.call_async(instant_orderRequest)
            rclpy.spin_until_future_complete(self, future, timeout_sec=10.0)

            if not future.done():
                self.get_logger().error("Service call timed out!")
                return
                
            t2 = datetime.datetime.now()
            self.get_logger().info("call instant_action finish! take " + str(t2-t1))
            if future.result().success == True:
                self.get_logger().info("instant order send success!!!")
            else:
                self.get_logger().error("instant order send failed!!!")

        except Exception as e:
            self.get_logger().error(str(e))

    def connection_handle(self, connection_msg):
        if connection_msg:
            self.time_monitor_old = datetime.datetime.now()

    def localization_handle(self,localization_msg):
        #self.get_logger().info("1111111111111111111111111111111111")
        request = Relocate.Request()
        #request.position_region = []
        try:
            # self.node.wait_for_service("agv_relocate", 2.0)
            if not self.relocateClient.wait_for_service(timeout_sec=5.0):
                self.get_logger().error("Service 'agv_relocate' not available")
                return

            if localization_msg['localization']['mode'] == 1:
                request.mode.data = 1
                request.point_yaw.x = localization_msg['localization']['pointYaw']['x']
                request.point_yaw.y = localization_msg['localization']['pointYaw']['y']
                request.point_yaw.theta = localization_msg['localization']['pointYaw']['theta']
                # value = localization_msg['localization'].get('zoneTag')
                if "zoneTag" in localization_msg['localization']:
                    request.zoneTag.data = localization_msg['localization']['zoneTag']
                else:
                    request.zoneTag.data = ''
                t1 = datetime.datetime.now()
                self.get_logger().info("call localization!!!")
                ret = self.relocateClient.call_async(request)
                t2 = datetime.datetime.now()
                self.get_logger().info("call localization finish! take " + str(t2-t1))
                #self.get_logger().info(ret)


            elif localization_msg['localization']['mode'] == 2:
                request.mode.data = 2
                request.position_radius.point.x = localization_msg['localization']['positionRadius']['x']
                request.position_radius.point.y = localization_msg['localization']['positionRdius']['y']
                request.position_radius.radius = localization_msg['localization']['positionRadius']['radius']
                ret = self.relocateClient.call_async(request)
                #self.get_logger().info(ret)

            elif localization_msg['localization']['mode'] == 3:
                request.mode.data = 3
                for point in localization_msg['localization']['positionRegion']:
                    pose = Point()
                    pose.x = point['x']
                    pose.y = point['y']
                    request.position_region.append(copy.deepcopy(pose))
                ret = self.relocateClient.call_async(request)
                #self.get_logger().info(ret)

            else:
                self.get_logger().error("relocate mode is wrong ! please check !!!")
            #self.get_logger().info(request)
        except Exception as e:
            self.get_logger().info(str(e))

    def qr_localization_handle(self,qr_localization_msg):
        request = LandmarkMap.Request()
        #self.get_logger().info("#"*100)
        try:
            # self.node.wait_for_service("landmark_service", 2.0)
            if not self.QRlocateClient.wait_for_service(timeout_sec=5.0):
                self.get_logger().error("Service 'landmark_service' not available")
                return

            if True:
                request.trigger = True
                self.get_logger().info("call qr_localization!!!")
                t1 = datetime.datetime.now()
                QRret = self.QRlocateClient.call_async(request)
                t2 = datetime.datetime.now()
                self.get_logger().info("call qr_localization finish! take " + str(t2-t1))
                #self.get_logger().info(QRret)
                self.qrLocation['id'] = QRret.id
                self.qrLocation['x'] = QRret.x
                self.qrLocation['y'] = QRret.y
                self.qrLocation['theta'] = QRret.yaw
                self.qrLocation_json = json.dumps(self.qrLocation)
                ret = self.mqtt_client.publish('/'+ self.agv_num +'/'+ QR_LOCALIZATION_RESPONSE_TOPIC,self.qrLocation_json)
            else:
                self.get_logger().error("QRlocate Error! please check !!!")
            #self.get_logger().info(request)
        except Exception as e:
            self.get_logger().info(str(e))

    def pc_switch_handle(self,pc_switch_msg):
        pc_switch_ = PointCloudPubSwitch()
        #self.get_logger().info(pc_switch_msg)
        try:
            pc_switch_.pc_pub_switch = pc_switch_msg['pc_pub_switch']
            pc_switch_.map_pub_switch = pc_switch_msg['map_pub_switch']
            self.pc_switch_pub.publish(pc_switch_)
            self.get_logger().info(pc_switch_.pc_pub_switch,pc_switch_.map_pub_switch)
        except:
            self.get_logger().info("point cloud switch message is wrong!")

    def save_map_handle(self,save_map_msg):
        try:
            if not self.saveMapClient.wait_for_service(timeout_sec=5.0):
                self.get_logger().error("Service 'save_map' not available")
            return
            self.get_logger().info(save_map_msg)
            save_map_ = SaveMap.Request()
            save_map_.name = save_map_msg['name']
            #save_map_.open = 0
            # self.node.wait_for_service("save_map", 2.0)
            
            t1 = datetime.datetime.now()
            self.get_logger().info("call save_map!!!")
            ret = self.saveMapClient.call_async(save_map_)
            t2 = datetime.datetime.now()
            self.get_logger().info("call save_map finish! take " + str(t2-t1))

            self.save_map_ret = {"status":0,"message":""}
            self.save_map_ret["status"] = ret.status.code
            self.save_map_ret["message"] = ret.status.message
            self.save_map_ret_json = json.dumps(self.save_map_ret)
            ret_ = self.mqtt_client.publish('/'+ self.agv_num +'/'+ SAVE_MAP_RESPONSE_TOPIC,self.save_map_ret_json)
            self.get_logger().info(ret.status.message)

        except Exception as e:
            self.get_logger().info(str(e))
            self.get_logger().info("save map failed!")

    def nav_mode_handle(self,nav_mode_msg):
        try:
            if not self.navModeClient.wait_for_service(timeout_sec=5.0):
                self.get_logger().error("Service 'location_mode' not available")
            return

            nav_mode_ = SetLoactionMode.Request()
            nav_mode_.mode = nav_mode_msg['navMode']
            if nav_mode_.mode == 0:
              if nav_mode_msg['mapUpdate'] == 1:
                  nav_mode_.mapId = nav_mode_msg['mapName']
              if nav_mode_msg['mapUpdate'] == 0:
                  nav_mode_.mapId = ""
              else:
                self.get_logger().info("mapUpdate msg wrong! please check!")
            if nav_mode_.mode == 1:
              nav_mode_.mapId = nav_mode_msg['mapName']
            nav_mode_.resolution = nav_mode_msg['ratio']
            # self.node.wait_for_service("location_mode", 2.0)
            
            t1 = datetime.datetime.now()
            self.get_logger().info("call nav_mode!!!")
            ret = self.navModeClient.call_async(nav_mode_)
            t2 = datetime.datetime.now()
            self.get_logger().info("call nav_mode finish! take " + str(t2-t1))

            self.nav_mode_ret = {"status":0,"message":""}
            self.nav_mode_ret["status"] =  ret.status.code
            self.nav_mode_ret["message"] = ret.status.message
            self.nav_mode_ret_json = json.dumps(self.nav_mode_ret)
            ret_ = self.mqtt_client.publish('/'+ self.agv_num +'/'+ NAV_MODE_RESPONSE_TOPIC,self.nav_mode_ret_json)
            self.get_logger().info(ret.status.message)

        except Exception as e:
            self.get_logger().info(str(e))
            self.get_logger().info("nav mode change failed!")

    def map_modify_handle(self,msg):
        #self.get_logger().info(msg)
        try:
            if not self.mapEditClient.wait_for_service(timeout_sec=5.0):
                self.get_logger().error("Service 'edit_map' not available")
            return

            map_modify_ = EditMap.Request()
            map_modify_.mapID = msg['mapId']
            areas_list_ = mapEdit_earse()
            point_ = mapEdit_points()
            for area in msg["areaList"]:
                #self.get_logger().info(area)
                area_ = mapEdit_poly()
                for point in area["area"]["points"]:
                    #self.get_logger().info(point)
                    point_.x = point["point"]["x"]
                    point_.y = point["point"]["y"]
                    area_.apex_points.append(copy.deepcopy(point_))
                area_.attribute = area["area"]["modifyMode"]
                areas_list_.apex_poly.append(copy.deepcopy(area_))
            areas_list_.operate = 1
            map_modify_.earse_action = areas_list_
            map_modify_.transform_action.operate = msg["transform"]["operate"]
            map_modify_.transform_action.angle = msg["transform"]["angle"]
            map_modify_.transform_action.x = msg["transform"]["x"]
            map_modify_.transform_action.y = msg["transform"]["y"]

            self.get_logger().info(map_modify_)
            # self.node.wait_for_service("edit_map", 2.0)
            self.get_logger().info("call map_modify!!!")
            t1 = datetime.datetime.now()
            ret = self.mapEditClient.call_async(map_modify_)
            t2 = datetime.datetime.now()
            self.get_logger().info("call map_modify finish! take " + str(t2-t1))
            self.map_modify_ret = {"status":0,"message":""}
            self.map_modify_ret["status"] = ret.status.code
            self.map_modify_ret["message"] = ret.status.message

            self.map_modify_ret_json = json.dumps(self.map_modify_ret)
            ret_ = self.mqtt_client.publish('/'+ self.agv_num +'/'+ MAP_MODIFY_RESPONSE_TOPIC,self.map_modify_ret_json)
            #self.get_logger().info(ret.status)


        except Exception as e:
            self.get_logger().info(str(e))
            self.get_logger().info("modify map failed!")

    def update_qr_handle(self,msg):
        try:
            if not self.mapEditClient.wait_for_service(timeout_sec=5.0):
                self.get_logger().error("Service 'updateqr' not available")
            return
            update_ = UpdateQr.Request()
            update_.trigger = 1
            # self.node.wait_for_service("updateqr", 2.0)
            self.get_logger().info("call update_qr!!!")
            t1 = datetime.datetime.now()
            ret = self.updateQR_client.call_async(update_)
            t2 = datetime.datetime.now()
            self.get_logger().info("call update_qr finish! take " + str(t2-t1))

            #self.get_logger().info(ret)
        except Exception as e:
            self.get_logger().info(str(e))
            self.get_logger().info("nav mode change failed!")

    def map_merge_handle(self,msg):
        try:
            if not self.mapEditClient.wait_for_service(timeout_sec=5.0):
                self.get_logger().error("Service 'merge_map' not available")
            return
            map_merge_ = StartMapMerge.Request()
            map_merge_.map_id_0 = msg['mapId0']
            map_merge_.map_id_1 = msg['mapId1']
            map_merge_.map_id_2 = msg['mapId2']
            # self.node.wait_for_service("merge_map", 2.0)
        
            self.get_logger().info("call map_merge!!!")
            t1 = datetime.datetime.now()
            ret = self.mapMerge_client.call_async(map_merge_)
            t2 = datetime.datetime.now()
            self.get_logger().info("call map_merge finish! take " + str(t2-t1))


            self.map_merge_ret = {"status":0,"message":""}
            self.map_merge_ret["status"] = ret.status.code
            self.map_merge_ret["message"] = ret.status.message

            self.map_merge_ret_json = json.dumps(self.map_merge_ret)
            ret_ = self.mqtt_client.publish('/'+ self.agv_num +'/'+ MAP_MODIFY_RESPONSE_TOPIC,self.map_merge_ret_json)
            self.get_logger().info(ret.status)

        except Exception as e:
            self.get_logger().info(str(e))
            self.get_logger().info("map merge failed!")


    def mqtt_cb(self, mqttself, client, message):
        # possibly add decisions to cancel current orders for instant actions.
        #self.get_logger().info(message.payload.decode('utf8'))
        #self.get_logger().info(type(message.payload.decode('utf8')))
        #self.get_logger().info("mqtt_cb:"+self.agv_num+",topic:"+message.topic)
        if message.topic == '/'+ self.agv_num + '/' + ORDER_TOPIC:
            # if self.verify_message(json.loads(message.payload.decode('utf8')), ORDER_SCHEMA,from_file = False) == False: return # If the message isn't formatted correctly it will be discarded
            #self.get_logger().info("Order message has be verified!")
            self.order_handle(json.loads(message.payload.decode('utf8')))

        elif message.topic == '/'+ self.agv_num + '/' + INSTANT_ACTION:
            if self.verify_message(json.loads(message.payload.decode('utf8')), INSTANT_ACTION_SCHEMA,from_file = False) == False: return # If the message isn't formatted correctly it will be discarded
            #self.get_logger().info("Instant action message has be verified!")
            self.instant_action_handle(json.loads(message.payload.decode('utf8')))

        elif message.topic == '/'+ self.agv_num + '/' + CONNECTION_TOPIC:
            if self.verify_message(json.loads(message.payload.decode('utf8')), CONNECTION_SCHEMA,from_file = False) == False: return # If the message isn't formatted correctly it will be discarded
            # self.get_logger().info("connection message has be verified!")
            self.connection_handle(json.loads(message.payload.decode('utf8')))

        elif message.topic == '/'+ self.agv_num + '/' + LOCALIZATION_TOPIC:
            #if self.verify_message(json.loads(message.payload.decode('utf8')), CONNECTION_SCHEMA,from_file = False) == False: return # If the message isn't formatted correctly it will be discarded
            #self.get_logger().info("connection message has be verified!")
            self.localization_handle(json.loads(message.payload.decode('utf8')))

        elif message.topic == '/'+ self.agv_num + '/' + QR_LOCALIZATION_REQUEST_TOPIC:
                #if self.verify_message(json.loads(message.payload.decode('utf8')), CONNECTION_SCHEMA,from_file = False) == False: return # If the message isn't formatted correctly it will be discarded
            #self.get_logger().info("qr message has be verified!")
            self.qr_localization_handle(json.loads(message.payload.decode('utf8')))

        elif message.topic == '/'+ self.agv_num + '/' + POINT_CLOUD_SWITCH:
            self.pc_switch_handle(json.loads(message.payload.decode('utf8')))

        elif message.topic == '/'+ self.agv_num + '/' + SAVE_MAP_TOPIC:
            self.save_map_handle(json.loads(message.payload.decode('utf8')))

        elif message.topic == '/'+ self.agv_num + '/' + NAV_MODE_TOPIC:
            self.nav_mode_handle(json.loads(message.payload.decode('utf8')))

        elif message.topic == '/'+ self.agv_num + '/' + MAP_MODIFY_TOPIC:
            self.map_modify_handle(json.loads(message.payload.decode('utf8')))

        elif message.topic == '/'+ self.agv_num + '/' + UPDATEQR_TOPIC:
            self.update_qr_handle(json.loads(message.payload.decode('utf8')))

        elif message.topic == '/'+ self.agv_num + '/' + MAP_MERGE_TOPIC:
            self.map_merge_handle(json.loads(message.payload.decode('utf8')))

        else:
            self.get_logger().info("topic is wrong! please check!!!")

    def verify_message(self, msg_list, schema_list, from_file = False):
        """This function verifies the message generated when an order is created.

        Parameters
        ----------
        msg_list : List[dict]
            - This is a list of the json dicts that has to be sent as messages.

        schema_list : List[dict]
            - This is a list of json schemas that correspond to the msg_list. These are used to verify the message is setup correctly.

        Returns
        -------
        is_valid : bool
            - If true, the message is the correct format and the entries are of the right types. If not it will return false.
        """

        if type(msg_list) != list:
            msg_list = [msg_list]
        if type(schema_list) != list:
            schema_list = [schema_list]

        if from_file == True:
            for msg, schema in zip(msg_list, schema_list):
                with open('/home/k/work_test_ws/src/VDA5050_to_ROS-main/messages/' + msg, 'r',  encoding = 'utf8') as file:
                    msg_data = json.load(file)
                    file.close()
                with open('/home/k/work_test_ws/src/VDA5050_to_ROS-main/schemas/' + schema, 'r',  encoding = 'utf8') as file:
                    schema = json.load(file)
                    file.close()
                is_valid = jsonschema.Draft3Validator(schema = schema).is_valid(msg_data)
                return is_valid
        else:
            for msg, schema in zip(msg_list, schema_list):
                #with open(r'/home/k/work_test_ws/src/VDA5050_to_ROS-main/schemas/' + schema, 'r',  encoding = 'utf8') as file:
                with open(self.schemas_path + schema, 'r',  encoding = 'utf8') as file:
                    schema = json.load(file)
                    file.close()
                is_valid = jsonschema.Draft3Validator(schema = schema).is_valid(msg)
                return is_valid


    def scan_cb(self,msg):
        #self.get_logger().info("scan cb !!!!")
        self.point_list = []
        self.point_msg['stamp'] = msg.header.stamp.secs
        for point in point_cloud2.read_points(msg, skip_nans=True):
            point_ = {'x':0.0,'y':0.0}
            point_['x'] = point[0]
            point_['y'] = point[1]
            self.point_list.append(point_)
        self.point_msg['points'] = self.point_list

    def event_cb(self,msg):
        #self.get_logger().info("event cb !!!!")
        error ={'errorType':'','errorReferences':[{"referenceKey": "", "referenceValue": ""}],'errorDescription':'','errorLevel':''}
        error_list = []

        error["errorType"] =  msg.ecode
        error["errorDescription"] = msg.description
        error_list.append(copy.deepcopy(error))
        if self.description != '':
          error["errorType"] = 0
          error["errorDescription"] = self.description
          error_list.append(copy.deepcopy(error))
        self.state.errors = error_list


    def map_cb(self,msg):
        #self.get_logger().info("map cb !!!!")
        map_info_bytes = ''
        img_bytes = ''
        if self.map_pub_switch == True and self.navigation_mode == 0:
            self.map_msg['stamp'] = msg.header.stamp.secs
            self.map_msg['resolution'] = msg.info.resolution
            self.map_msg['width'] = msg.info.width
            self.map_msg['height'] = msg.info.height
            self.map_msg['origin']['position']['x'] = msg.info.origin.position.x
            self.map_msg['origin']['position']['y'] = msg.info.origin.position.y

            map_info_bytes = bytes('{}'.format(self.map_msg),'utf-8') + bytes(r'#%#','utf-8')
            # self.map_msg['data'] = msg.data

            #self.get_logger().info(msg.info.width,msg.info.height)

            img = np.array(msg.data,np.uint8)
            img = img.reshape(msg.info.height,msg.info.width)
            img = cv2.flip(img,0)
            self.i = self.i + 1
            ret = cv2.imwrite('map.png',img,[int(cv2.IMWRITE_PNG_COMPRESSION),5])

            if ret == True:
                with open('map.png','rb') as f:
                    img_bytes = base64.b64encode(f.read()) #bytes

        else:
            pass

        self.send_info = map_info_bytes + img_bytes

    def switch_cb(self,msg):
        if msg.pc_pub_switch == True:
            self.pc_pub_switch = True
        else:
            self.pc_pub_switch = False

        if msg.map_pub_switch == True:
            self.map_pub_switch = True
        else:
            self.map_pub_switch = False


    def state_json(self):
        #with open(self.msg_path +'state_msg.json', 'r', encoding = 'utf8') as json_file:
            # Open json schema as a template for creating the order.
            #json_object = json.load(json_file)
            #json_file.close()
            #self.get_logger().info(json_object)

            # Fill out the header for the order using the following parameters
            self.json_object['headerId'] = self.state.headerId
            self.json_object['orderId'] = self.state.orderId
            self.json_object['orderUpdateId'] = self.state.orderUpdateId
            self.json_object['manufacturer'] = self.state.manufacturer
            self.json_object['serialNumber'] = self.state.serialNumber
            self.json_object['timestamp'] = datetime.datetime.now().isoformat()
            self.json_object['lastNodeId'] = self.state.lastNodeId
            self.json_object['lastNodeSequenceId'] = self.state.lastNodeSequenceId
            self.json_object['driving'] = self.state.driving
            self.json_object['operatingMode'] = self.state.operatingMode
            self.json_object['velocity'] = self.state.velocity
            self.json_object['agvPosition'] = self.state.agvPosition
            self.json_object['paused'] = self.state.paused
            self.json_object['distanceSinceLastNode'] = self.state.distanceSinceLastNode
            self.json_object['actionStates'] = self.state.actionStates
            self.json_object['edgeStates'] = self.state.edgeStates
            self.json_object['newBaseRequest'] = self.state.newBaseRequest
            self.json_object['errors'] = self.state.errors
            self.json_object['batteryState'] = self.state.batteryStates
            self.json_object['loads'] = self.state.loads
            #self.json_object['agvEvent'] = self.state.agvEvents
            self.json_object['centerMileage'] = self.state.center_mileage
            self.json_object['onlineTime'] = self.state.online_time
            self.json_object['moveTime'] = self.state.move_time
            self.json_object['faultTime'] = self.state.fault_time
            self.json_object['orderCounts'] = self.state.order_counts
            self.json_object['dischargeTime'] = self.state.discharge_time
            self.json_object['chargeCounts'] = self.state.charge_counts
            self.json_object['chargeTime'] = self.state.charge_time


            try:
                self.heart_bit_state = json.dumps(self.json_object)
                #self.get_logger().info(ret)
                #return ret
            except Exception as e:
                self.get_logger().info(str(e))
                self.get_logger().info("json convert failed !!!")

    def publish_point_cloud(self):
        if self.pc_pub_switch == True:
            self.get_logger().info("publish_point !!!!")
            try:
                publish_data = json.dumps(self.point_msg)
                #publish_data = publish_data + r'$$$'
            except:
                self.get_logger().info("convert points to json file faild!!!")
            try:
                self.mqtt_client.publish('/'+ self.agv_num +'/'+ POINT_CLOUD_TOPIC,publish_data)
            except Exception as e:
                self.get_logger().info(str(e))
                self.get_logger().info("send1 faild")
        else:
            pass

    def publish_map(self):
        self.navigation_mode = self.get_parameter('localization').value
        if self.map_pub_switch == True and self.navigation_mode == 0:
            #self.get_logger().info("publish_map !!!!")
            try:
                self.mqtt_client.publish('/'+ self.agv_num +'/'+ MAP_TOPIC,self.send_info)
                time_ = time.strftime("%Y-%m-%d %X", time.localtime())
                #self.get_logger().info(time_)
            except Exception as e:
                self.get_logger().info(str(e))
                self.get_logger().info("send2 faild")
        else:
            # self.nav_mode_ret = {"status":0,"message":""}
            # self.nav_mode_ret["status"] =  -1
            # self.nav_mode_ret["message"] = "map date is only published in mapping mode!"
            # self.nav_mode_ret_json = json.dumps(self.nav_mode_ret)
            # ret_ = self.mqtt_client.publish('/'+ self.agv_num +'/'+ NAV_MODE_RESPONSE_TOPIC,self.nav_mode_ret_json)
            self.send_info = "map date is only published in mapping mode!"
            ret_ = self.mqtt_client.publish('/'+ self.agv_num +'/'+ MAP_TOPIC,self.send_info)

    def publish_error(self):
        self.event_msg = AGVEvent()
        #self.event_msg.header.seq = self.seq + 1
        self.event_msg.header.stamp = self.get_clock().now().to_msg()
        self.event_msg.header.frame_id = self.get_name()
        self.event_msg.ecode = 0
        self.event_msg.type = 0
        self.event_msg.action = 0
        self.event_msg.eventtime = self.get_clock().now().nanoseconds / 1e9
        if self.state.operatingMode != MANUAL_OP_MODE:
            if self.broker_disconnect == 1:
                self.event_msg.description = "broker connect error!"
                self.event_msg.ecode = AGVEvent.NETWORK_FAIL_EVENT
                self.event_msg.type = 2
                self.event_msg.action = 3
            elif self.master_disconnect == 1:
                self.event_msg.description = "master connect error!"
                self.event_msg.ecode = AGVEvent.NETWORK_FAIL_EVENT
                self.event_msg.type = 2
                self.event_msg.action = 3

        self.error_pub.publish(self.event_msg)

def main(args=None):
    rclpy.init(args=args)
    
    node = TaskManager()
    executor = MultiThreadedExecutor()
    executor.add_node(node)
    
    try:
        executor.spin()
    except KeyboardInterrupt:
        pass
    finally:
        node.mqtt_client.loop_stop()
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()